/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "jogcontrol.h"

JogControl::JogControl(TrajectoryServer *trajServerIn, RobotCommonParameter robotParameterIn, MotionMessage *motionMessageIn)
{
    motionMessage=motionMessageIn;
    trajServer=trajServerIn;
    robotParameter=robotParameterIn;
    robotId=robotParameter.robotId;
    jointNum=robotParameter.jointNum;
    internalNum=robotParameter.armControlAxisNumber;
    externalNum=robotParameter.externalNum;
    currentJogCoordinate=ENUM_JOINT;
    allowSingleAxisJogFlag=false;
}

int JogControl::setRobotCoordinateConfig(RobotCoordinateAxisConfig coordinateConfigIn)
{
    robotParameter.jogDecartLineRatio=coordinateConfigIn.jogDecartLineRatio;
    robotParameter.jogDecartRotateRation=coordinateConfigIn.jogDecartRotateRation;
    return 1;
}


int JogControl::allowSingleAxisJog(bool enable)
{
    qDebug()<<"JogControl::allowSingleAxisJog, enable="<<enable;
    allowSingleAxisJogFlag=enable;
    return 1;
}

int JogControl::moveToPoint(const MoveParameter target)//移动到点没有滚轮调速的功能
{

    MoveParameter tmpCommand=target;
    QVector<MoveParameter> tmpCommandVector;
    tmpCommand.velocity=robotParameter.jogDecartLineRatio;
//    tmpCommand.lineNumber=ENUM_LINE_MOVETOPOINT;
    tmpCommandVector.append(tmpCommand);
    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->setMoveToPointFlag();
    trajServer->addTrajectoryCommand(tmpCommandVector);
    trajServer->trajectoryRun();
    trajServer->setBasicStatus(ENUM_TRAJECTORY_RUN);


    return 1;


}

int JogControl::getTargetPoint(int axis, int direction,int coordinateType,PointPro &targetPoint)
{
    //user tool
    PointPro startPoint;
    trajServer->getCurrentCoordinatePoint_Feedback(startPoint);
    Frame startPointFrame,transFrame,targetFrame;
    transFrame=Frame::Identity();

    //world
    Frame transFrame1, transFrame2, transFrame3;
    trajServer->getCurrentUserInverseMatrix(transFrame1);
    transFrame1.p[0]=0;
    transFrame1.p[1]=0;
    transFrame1.p[2]=0;
    transFrame2=Frame::Identity();


//    startPointFrame =trajServer->pointProToFrame(startPoint);
    if(1!=trajServer->pointProToFrame(startPoint,startPointFrame))
    {
        qDebug()<< "error,,,,,jogForbbb";
        return -1;
    }

    //搜索极限位置
    int  tmpFindCount=0;
    int isSucess=-1;
    PointPro tmpPoint;
    double tmpStepLength;
    double recordPos=0;
    double lastSucessPos=0;
    if(1==direction)
    {
        tmpStepLength=JOG_SEGMENT_LENGTH_LINE/1000.0;
    }
    else
    {
        tmpStepLength=-JOG_SEGMENT_LENGTH_LINE/1000.0;
    }
    recordPos=tmpStepLength;
//    transFrame.p[axis]=startPointFrame.p[axis]+tmpStepLength;
    transFrame.p[axis]+=tmpStepLength;

//    transFrame2.p[axis]=transFrame2.p[axis]+tmpStepLength;
    transFrame2.p[axis]+=tmpStepLength;

    while(tmpFindCount<JOG_FIND_MAX_COUNT)
    {
        switch(coordinateType)
        {
        case ENUM_TOOL:
            targetFrame=startPointFrame*transFrame;
            break;
        case ENUM_USER:
            targetFrame=transFrame*startPointFrame;
            break;
        case ENUM_WORLD:
            transFrame3=transFrame1*transFrame2;
            targetFrame=transFrame3*startPointFrame; //得到转移后的xyz,姿态不对????
            targetFrame.M=startPointFrame.M;
            break;
        }

        targetPoint=trajServer->frameToPointPro_CART(targetFrame);
        targetPoint.configString=startPoint.configString;

        if(1==trajServer->pointCoordinateToJoint_noDeviationCheck(targetPoint,tmpPoint,false))
        {
            lastSucessPos=recordPos;
            isSucess=1;
            qDebug()<<tmpFindCount<<" +recordPos"<<recordPos<<"tmpStepLength"<<tmpStepLength;
            if(fabs(tmpStepLength)<JOG_FIND_QULIFIY_DELTA_DISTANCE)
            {
                qDebug()<<tmpFindCount<<"提前搜索到位置"<<recordPos<<"tmpStepLength"<<tmpStepLength
                       <<"startPointFrame"<<startPointFrame.p[0]<<startPointFrame.p[1]
                      <<startPointFrame.p[2]<<"targetFrame"<<targetFrame.p[0]<<targetFrame.p[1]
                     <<targetFrame.p[2];
                break;
            }
            tmpStepLength=tmpStepLength/2.0;
            recordPos=recordPos+tmpStepLength;
        }
        else
        {
            if(tmpFindCount==JOG_FIND_MAX_COUNT-2)//可能出现临界点附近一直搜索不成功。
            {
                recordPos=lastSucessPos;
                qDebug()<<"临界点附近 tmpFindCount==JOG_FIND_MAX_COUNT-2 recordPos"<<recordPos;
            }
            else
            {
                tmpStepLength=tmpStepLength/2.0;
                recordPos=recordPos-tmpStepLength;
            }

            isSucess=-1;
            qDebug()<<tmpFindCount<<" -recordPos"<<recordPos<<"targetFrame"
                   <<targetFrame.p[0]<<targetFrame.p[1]<<targetFrame.p[2]<<"tmpStepLength"<<tmpStepLength;
        }

        if(ENUM_WORLD==coordinateType)
        {
            transFrame2=Frame::Identity();
            transFrame2.p[axis]+=recordPos;
        }
        else
        {
            transFrame=Frame::Identity();
            transFrame.p[axis]+=recordPos;
        }

        tmpFindCount++;
    }
    return isSucess;
}

int JogControl::jog(int axis, int direction,int velocityOpt)
{

    switch (currentJogCoordinate)
    {
        case ENUM_JOINT:
        {
            jogForJoint(axis, direction,velocityOpt);
            break;
        }
        case ENUM_TOOL:
        {
            if(3>axis)
            {
                jogForToolCoordinateTranslate(axis, direction,velocityOpt);
            }
            else
            {
                jogForToolCoordinateRotation(axis, direction,velocityOpt);
            }
            break;
        }
            case ENUM_USER:
            {
            if(3>axis)
            {
                jogForUserCoordinateTranslate(axis, direction,velocityOpt);
            }
            else
            {
                jogForUserCoordinateRotation(axis, direction,velocityOpt);
            }
            break;
        }
        case ENUM_WORLD:
        {
            if(3>axis)
            {
                jogForWorldCoordinateTranslate(axis, direction,velocityOpt);
            }
            else
            {
                jogForWorldCoordinateRotation(axis, direction,velocityOpt);
            }
            break;
        }
    }

    return 1;

}

int JogControl::jogForJoint(int axis, int direction,int velocityOpt)
{
    if(axis>=robotParameter.jointNum)
    {
        qDebug()<<"JogControl::jog error;axis>=robotParameter.jointNum";
        return -1;
    }

    if(trajServer->isAxisZeroOk()==false && allowSingleAxisJogFlag==true)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====isAxisZeroOk()==false, warnning";
        }

        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 7401, robotId);
        trajServer->allowSingleAxisJog(true);
    }

    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;

    trajServer->getCurrentJointPoint_Feedback(targetPoint);
    if(axis<robotParameter.jointNum && axis<targetPoint.positionValue.size())
    {
        if(1==direction)
        {
            targetPoint.positionValue[axis]+=JOG_SEGMENT_LENGTH_ANGULAR;
        }
        else
        {
           targetPoint.positionValue[axis]-=JOG_SEGMENT_LENGTH_ANGULAR;
        }
    }
    else
    {

        qDebug()<< "JogControl===jogForJoint error, axis>=jointNum";

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7403, robotId,axis);
        return -1;

    }

    targetPoint.extPositionValue.resize(0);

    tmpMovePara.moveType = ENUM_COMMAND_JOINT_TRAP;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 1;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber =ENUM_LINE_JOG;
    targetPoint.jointFlag=JOINT_FLAG_JOINT;
    tmpMovePara.targetPoint = targetPoint;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
//    trajServer->trajectoryRun();

    return 1;
}

int JogControl::jogForToolCoordinateTranslate(int axis, int direction,int velocityOpt)
{
    if(trajServer->isAxisZeroOk()==false)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7400, robotId);
        return -1;
    }

    PointPro targetPoint;

    if(1!=getTargetPoint(axis,direction,ENUM_TOOL,targetPoint))
    {
        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7404, robotId);
        return -1;
    }



    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = robotParameter.jogDecartLineRatio;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber = ENUM_LINE_JOG;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
//    trajServer->trajectoryRun();

   return 1;
}

int JogControl::jogForToolCoordinateRotation(int axis, int direction,int velocityOpt)
{
    if(trajServer->isAxisZeroOk()==false)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7400, robotId);
        return -1;
    }
    PointPro startPoint,targetPoint;
    trajServer->getCurrentCoordinatePoint_Feedback(startPoint);
    Frame startPointFrame,targetPointFrame;
//    startPointFrame =trajServer->pointProToFrame(startPoint);
    if(1!=trajServer->pointProToFrame(startPoint,startPointFrame))
    {
        qDebug()<< "error,,,,,jogForToolCoordinateRotation";
        return -1;
    }
    Frame transFrame=Frame::Identity();
    if(1==direction)
    {
        if (axis==3)
        {
            transFrame.M.DoRotX(JOG_ROTATION_ANGULAR);
        }//X轴自旋
        else if (axis==4)
        {
            transFrame.M.DoRotY(JOG_ROTATION_ANGULAR);
        }//Y轴自旋
        else if (axis==5)
        {
            transFrame.M.DoRotZ(JOG_ROTATION_ANGULAR);
        }//Z轴自旋
    }
    else
    {
        if (axis==3)
        {
            transFrame.M.DoRotX(-JOG_ROTATION_ANGULAR);
        }//X轴自旋
        else if (axis==4)
        {
            transFrame.M.DoRotY(-JOG_ROTATION_ANGULAR);
        }//Y轴自旋
        else if (axis==5)
        {
            transFrame.M.DoRotZ(-JOG_ROTATION_ANGULAR);
        }//Z轴自旋
    }
    targetPointFrame=startPointFrame*transFrame;
    targetPoint=trajServer->frameToPointPro_CART(targetPointFrame);

    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = robotParameter.jogDecartRotateRation;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber = ENUM_LINE_JOG;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
//    trajServer->trajectoryRun();
    return 1;
}


int JogControl::jogForWorldCoordinateTranslate(int axis, int direction,int velocityOpt)
{
     qDebug()<< "JogControl====jogForWorldCoordinateTranslate"<<axis<<direction<<velocityOpt;
    if(trajServer->isAxisZeroOk()==false)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7400, robotId);
        return -1;
    }


    PointPro targetPoint;

    if(1!=getTargetPoint(axis,direction,ENUM_WORLD,targetPoint))
    {
        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7404, robotId);
        return -1;
    }


    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = robotParameter.jogDecartLineRatio;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber = ENUM_LINE_JOG;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
//    trajServer->trajectoryRun();

   return 1;
}

int JogControl::jogForWorldCoordinateRotation(int axis, int direction,int velocityOpt)
{
    qDebug()<< "JogControl====jogForWorldCoordinateRotation"<<axis<<direction<<velocityOpt;
    if(trajServer->isAxisZeroOk()==false)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7400, robotId);
        return -1;
    }
    PointPro startPoint,targetPoint;
    trajServer->getCurrentCoordinatePoint_Feedback(startPoint);
    Frame startPointFrame, targetPointFrame;
//    startPointFrame =trajServer->pointProToFrame(startPoint);
    if(1!=trajServer->pointProToFrame(startPoint,startPointFrame))
    {
        qDebug()<< "error,,,,,jogForToolCoordinate cc";
        return -1;
    }
    Frame transFrame1, transFrame2, transFrame3;
    trajServer->getCurrentUserInverseMatrix(transFrame1);
//    transFrame1.p[0]=0;
//    transFrame1.p[1]=0;
//    transFrame1.p[2]=0;
//    transFrame2=Frame::Identity();//必须使用单元矩阵。

    if(1==direction)
    {
        if (axis==3)
        {
            transFrame2.M.DoRotX(JOG_ROTATION_ANGULAR);
        }//X轴自旋
        else if (axis==4)
        {
            transFrame2.M.DoRotY(JOG_ROTATION_ANGULAR);
        }//Y轴自旋
        else if (axis==5)
        {
            transFrame2.M.DoRotZ(JOG_ROTATION_ANGULAR);
        }//Z轴自旋
    }
    else
    {
        if (axis==3)
        {
            transFrame2.M.DoRotX(-JOG_ROTATION_ANGULAR);
        }//X轴自旋
        else if (axis==4)
        {
            transFrame2.M.DoRotY(-JOG_ROTATION_ANGULAR);
        }//Y轴自旋
        else if (axis==5)
        {
            transFrame2.M.DoRotZ(-JOG_ROTATION_ANGULAR);
        }//Z轴自旋
    }

    transFrame3=transFrame1*transFrame2;
    targetPointFrame=transFrame3*startPointFrame; //得到转移后的wpr,位置不对
    targetPointFrame.p=startPointFrame.p;

    targetPoint=trajServer->frameToPointPro_CART(targetPointFrame);
    for(int i=0;i<targetPoint.positionValue.size();i++)
    {
         qDebug()<<targetPoint.positionValue[i];
    }


    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = robotParameter.jogDecartRotateRation;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber = ENUM_LINE_JOG;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
//    trajServer->trajectoryRun();
    return 1;
}


int JogControl::jogForUserCoordinateTranslate(int axis, int direction,int velocityOpt)
{
    if(trajServer->isAxisZeroOk()==false)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7400, robotId);
        return -1;
    }

    PointPro targetPoint;

    if(1!=getTargetPoint(axis,direction,ENUM_USER,targetPoint))
    {
        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7404, robotId);
        return -1;
    }



    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = robotParameter.jogDecartLineRatio;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber = ENUM_LINE_JOG;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
//    trajServer->trajectoryRun();

   return 1;
}

int JogControl::jogForUserCoordinateRotation(int axis, int direction,int velocityOpt)
{
    if(trajServer->isAxisZeroOk()==false)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7400, robotId);
        return -1;
    }
    PointPro startPoint,targetPoint;
    trajServer->getCurrentCoordinatePoint_Feedback(startPoint);
    Frame startPointFrame,targetPointFrame;
//    startPointFrame =trajServer->pointProToFrame(startPoint);
    if(1!=trajServer->pointProToFrame(startPoint,startPointFrame))
    {
        qDebug()<< "error,,,,,jogForccc";
        return -1;
    }
    Frame transFrame=Frame::Identity();
    if(1==direction)
    {
//        transFrame.p[axis]=startPointFrame.p[axis]+JOG_SEGMENT_LENGTH_LINE;
        if (axis==3)
        {
            transFrame.M.DoRotX(JOG_ROTATION_ANGULAR);
        }//X轴自旋
        else if (axis==4)
        {
            transFrame.M.DoRotY(JOG_ROTATION_ANGULAR);
        }//Y轴自旋
        else if (axis==5)
        {
            transFrame.M.DoRotZ(JOG_ROTATION_ANGULAR);
        }//Z轴自旋
    }
    else
    {
        if (axis==3)
        {
            transFrame.M.DoRotX(-JOG_ROTATION_ANGULAR);
        }//X轴自旋
        else if (axis==4)
        {
            transFrame.M.DoRotY(-JOG_ROTATION_ANGULAR);
        }//Y轴自旋
        else if (axis==5)
        {
            transFrame.M.DoRotZ(-JOG_ROTATION_ANGULAR);
        }//Z轴自旋
    }
    targetPointFrame=transFrame*startPointFrame;
    targetPointFrame.p=startPointFrame.p;//先旋转，再平移，位置会变。
    targetPoint=trajServer->frameToPointPro_CART(targetPointFrame);

    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = robotParameter.jogDecartRotateRation;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber = ENUM_LINE_JOG;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
//    trajServer->trajectoryRun();
    return 1;
}

int JogControl::getJogCoordinate()
{
    return currentJogCoordinate;
}

int JogControl::setJogCoordinate(int coordinateType)
{
    currentJogCoordinate=coordinateType;
    return 1;
}


int JogControl::returnZeroPoint(int axisNumber, int &returnFlag)
{
    if(trajServer->isAxisZeroOk()==false)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "JogControl====returnZeroPoint()==false, error";
        }

        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 7402, robotId);
        return -1;
    }

    if(axisNumber>=jointNum || axisNumber<-1)
    {

        qDebug()<< "JogControl====returnZeroPoint() error,axisNumber>=jointNum || axisNumber<-1";

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 7403, robotId,axisNumber);
        return -1;

    }

    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;

    trajServer->getCurrentJointPoint_Feedback(targetPoint);
    QVector<double> mechanicalZero=trajServer->getJointMechanicalZeroValue();
    //-1全部轴回零
    if(axisNumber==-1)
    {
        for(int i=0;i<jointNum;i++)
        {
            if(i<internalNum)
            {
                targetPoint.positionValue[i]=mechanicalZero[i];
            }
            else
            {
                targetPoint.extPositionValue[i-internalNum]=mechanicalZero[i];
            }

        }
    }
    else
    {//单轴回零
        if(axisNumber<internalNum)
        {
            targetPoint.positionValue[axisNumber]=mechanicalZero[axisNumber];
        }
        else
        {
            targetPoint.extPositionValue[axisNumber-internalNum]=mechanicalZero[axisNumber];
        }
    }



    tmpMovePara.moveType = ENUM_COMMAND_JOINT_TRAP;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 1;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.index = 0;
    tmpMovePara.cntRatio = 0;
    tmpMovePara.acc = 0;
    tmpMovePara.lineNumber =ENUM_LINE_JOG;
    targetPoint.jointFlag=JOINT_FLAG_JOINT;
    tmpMovePara.targetPoint = targetPoint;
    motionParaList.append(tmpMovePara);

    trajServer->clearTraj(true);//清除缓存轨迹
    trajServer->addTrajectoryCommand(motionParaList);
    trajServer->setMoveToPointFlag();
    trajServer->trajectoryRun();
    trajServer->setBasicStatus(ENUM_TRAJECTORY_RUN);
    returnFlag=0;
    return 1;
}




void JogControl::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn,
                                     int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;

    switch(messageCode)
    {
    case 7404:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("手动方向超出机械手工作范围!").toStdString();
       infomationStr = str1;
       break;
    }
    case 7403:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("手动轴号(%1)超出限制!").arg(parameter1).toStdString();
       infomationStr = str1;
       break;
    }
    case 7402:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("没有进行机器人零点校准，无法回机械零点!").toStdString();
       infomationStr = str1;
       break;
    }
    case 7401:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("没有进行机器人零点校准，可以关节手动，注意超过机械限位（系统此时没有限位保护）!").toStdString();
       infomationStr = str1;
       break;
    }
    case 7400:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("没有进行机器人零点校准，不允许坐标系手动，可以关节手动，注意超过机械限位（系统此时没有限位保护）!").toStdString();
       infomationStr = str1;
       break;
    }

        default:
        {
            qDebug()<<"CoordinateSolver::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
